/****************************************************************************
 *
 *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include "ActuatorEffectivenessHetero10PWM.hpp"
#include <lib/mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <uORB/topics/vehicle_local_position.h>

// ========================================
// 控制参数配置区域 - 方便调整
// ========================================

// 手动输入检测参数
const float MANUAL_DEADZONE = 0.05f;        // 手动输入死区阈值
const float YAW_INPUT_THRESHOLD = 0.05f;    // 偏航手动输入阈值

// X轴位置PID控制参数（前进/后退）
const float KP_X = 0.5f;                     // X轴比例增益
const float KI_X = 0.1f;                     // X轴积分增益
const float X_OUTPUT_LIMIT = 0.3f;           // X轴输出限幅

// Y轴位置PID控制参数（左/右移动）
const float KP_Y = 0.5f;                     // Y轴比例增益
const float KI_Y = 0.1f;                     // Y轴积分增益
const float Y_OUTPUT_LIMIT = 0.3f;           // Y轴输出限幅

// Z轴高度PID控制参数
const float KP_Z = 0.8f;                    // 高度比例增益
const float KI_Z = 0.2f;                    // 高度积分增益
const float Z_OUTPUT_LIMIT = 0.4f;          // 高度输出限幅

// 偏航(Yaw)PID控制参数
const float YAW_KP = 5.0f;                  // 偏航比例增益
const float YAW_KI = 0.5f;                  // 偏航积分增益
const float YAW_KD = 0.2f;                  // 偏航微分增益
const float YAW_INTEGRAL_LIMIT = 0.5f;       // 偏航积分限幅
const float YAW_OUTPUT_LIMIT = 1.0f;         // 偏航输出限幅

// 偏航控制模式参数
const float YAW_MANUAL_P_FACTOR = 0.5f;     // 手动偏航模式比例因子
const float YAW_MANUAL_D_FACTOR = 0.7f;     // 手动偏航模式微分因子
const float YAW_AUTO_P_FACTOR = 2.0f;       // 自动偏航模式比例因子
const float YAW_AUTO_D_FACTOR = 0.5f;       // 自动偏航模式微分因子

// 电机输出限制参数
const float MOTOR_MIN_OUTPUT = 0.0f;        // 电机最小输出
const float MOTOR_MAX_OUTPUT = 1.0f;        // 电机最大输出

// 测试电机参数
const float TEST_MOTOR9_OUTPUT = 0.8f;      // 9号电机测试输出值

// 调试信息输出频率控制
const int DEBUG_INFO_INTERVAL = 10;         // 基础调试信息输出间隔(调用次数)
const int POSITION_DEBUG_INTERVAL = 50;     // 位置控制调试信息输出间隔(调用次数)
const int ATTITUDE_DEBUG_INTERVAL = 200;    // 姿态控制调试信息输出间隔(调用次数)

// ========================================
// 控制逻辑实现
// ========================================

bool ActuatorEffectivenessHetero10PWM::getEffectivenessMatrix(Configuration &configuration,
		EffectivenessUpdateReason external_update)
{
	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
		return false;
	}

	// 选择矩阵0
	configuration.selected_matrix = 0;

	// 设置直通控制模式 - 不使用有效性矩阵
	// 在updateSetpoint中实现直接映射和PID控制
	configuration.effectiveness_matrices[configuration.selected_matrix].setIdentity();

	// 添加10个电机执行器和2个舵机执行器
	configuration.actuatorsAdded(ActuatorType::MOTORS, 10); // 索引0-9: 10个电机
	configuration.actuatorsAdded(ActuatorType::SERVOS, 2);  // 索引12-13: 2个舵机

	return true;
}

void ActuatorEffectivenessHetero10PWM::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
		    ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
{

	// 姿态稳定修正量
	float roll_correction = 0.0f;
	float pitch_correction = 0.0f;
	float yaw_correction = 0.0f;

	// 获取当前时间
	hrt_abstime current_time = hrt_absolute_time();
	float dt = 0.01f; // 默认时间步长

	// 计算时间差
	if (_last_time > 0) {
		dt = (current_time - _last_time) / 1e6f; // 转换为秒
		dt = math::constrain(dt, 0.001f, 0.1f);  // 限制时间步长范围
	}
	_last_time = current_time;

	// 读取姿态和角速度数据
	vehicle_attitude_s attitude{};
	vehicle_angular_velocity_s angular_velocity{};
	vehicle_local_position_s local_position{};

	bool has_attitude = _uorb_attitude_sub.copy(&attitude);
	bool has_angular_velocity = _uorb_angular_velocity_sub.copy(&angular_velocity);
	bool has_position = _uorb_local_position_sub.copy(&local_position);

	// 添加基础数据测试输出
	if (!has_attitude || !has_angular_velocity) {
		PX4_WARN("Hetero10PWM: 姿态数据不可用 attitude=%d angular=%d", (int)has_attitude, (int)has_angular_velocity);
	}

	// 位置控制逻辑
	float position_forward_output = 0.0f;  // X轴位置控制输出（前进）
	float position_lateral_output = 0.0f;   // Y轴位置控制输出（横移）
	float altitude_throttle_output = 0.0f;  // Z轴高度控制输出（油门）

	// 检查是否启用位置控制
	const bool has_valid_position = has_position && (hrt_elapsed_time(&local_position.timestamp) < 1000000); // 1秒 = 1000000微秒

	// 位置控制初始化
	if (has_valid_position && (fabsf(_target_x) < 0.001f && fabsf(_target_y) < 0.001f && fabsf(_target_z) < 0.001f)) {
		// 第一次获取有效位置时，设置当前位置为目标位置
		_target_x = local_position.x;
		_target_y = local_position.y;
		_target_z = local_position.z;
		_position_hold_enabled = true;
		_altitude_hold_enabled = true;
		PX4_INFO("位置控制初始化: 目标位置 X=%.2f Y=%.2f Z=%.2f",
			(double)_target_x, (double)_target_y, (double)_target_z);
	}

	// 检查手动输入，如果有则禁用位置控制
	float manual_roll = control_sp(ControlAxis::ROLL);
	float manual_pitch = control_sp(ControlAxis::PITCH);
	float manual_throttle = control_sp(ControlAxis::THRUST_Z);

	bool has_manual_input = (fabsf(manual_roll) > MANUAL_DEADZONE) ||
				(fabsf(manual_pitch) > MANUAL_DEADZONE) ||
				(fabsf(manual_throttle) > MANUAL_DEADZONE);

	// 位置控制逻辑 - 仅在无手动输入且位置数据有效时启用
	if (!has_manual_input && has_valid_position) {
		// X轴位置控制（前进/后退）
		float x_error = _target_x - local_position.x;
		_x_integral += KI_X * x_error * dt;
		_x_integral = math::constrain(_x_integral, -X_OUTPUT_LIMIT, X_OUTPUT_LIMIT);
		position_forward_output = KP_X * x_error + _x_integral;
		position_forward_output = math::constrain(position_forward_output, -X_OUTPUT_LIMIT, X_OUTPUT_LIMIT);

		// Y轴位置控制（左/右移动）
		float y_error = _target_y - local_position.y;
		_y_integral += KI_Y * y_error * dt;
		_y_integral = math::constrain(_y_integral, -Y_OUTPUT_LIMIT, Y_OUTPUT_LIMIT);
		position_lateral_output = KP_Y * y_error + _y_integral;
		position_lateral_output = math::constrain(position_lateral_output, -Y_OUTPUT_LIMIT, Y_OUTPUT_LIMIT);

		// Z轴高度PID控制
		// Z轴控制（高度）
		float z_error = _target_z - local_position.z;  // 注意：local_position.z是负高度
		_z_integral += KI_Z * z_error * dt;
		_z_integral = math::constrain(_z_integral, -Z_OUTPUT_LIMIT, Z_OUTPUT_LIMIT);
		altitude_throttle_output = KP_Z * z_error + _z_integral;
		altitude_throttle_output = math::constrain(altitude_throttle_output, -Z_OUTPUT_LIMIT, Z_OUTPUT_LIMIT);

		// 输出调试信息
		static int position_debug_counter = 0;
		if (++position_debug_counter % POSITION_DEBUG_INTERVAL == 0) {
			PX4_INFO("位置控制: X误差=%.3f Y误差=%.3f Z误差=%.3f 前进输出=%.3f 横移输出=%.3f 油门输出=%.3f",
				(double)x_error, (double)y_error, (double)z_error,
				(double)position_forward_output, (double)position_lateral_output, (double)altitude_throttle_output);
		}
	}

	// Yaw PID控制逻辑 - 自定义实现避免PID库编译错误
	if (has_attitude && has_angular_velocity && dt > 0.001f) {
		// 从四元数提取当前yaw角度
		matrix::Eulerf euler_angles(matrix::Quatf(attitude.q));
		float current_yaw = euler_angles.psi(); // 当前yaw角度

		// 检查是否需要重置目标角度（使用容差避免浮点数比较）
		if (fabsf(_last_yaw) < 0.001f) {
			_last_yaw = current_yaw;  // 第一次运行时设置初始目标
		}

		// 目标yaw角度 - 保持初始角度作为目标
		float target_yaw = _last_yaw;

		// 计算yaw角度误差（处理角度环绕）
		float yaw_error = matrix::wrap_pi(target_yaw - current_yaw);

		// 获取当前yaw角速度
		float current_yaw_rate = angular_velocity.xyz[2]; // z轴角速度

		// 自定义PID控制实现 - 使用定义的参数
		// 比例项
		float p_term = YAW_KP * yaw_error;

		// 积分项
		_yaw_integral += YAW_KI * yaw_error * dt;
		if (fabsf(_yaw_integral) > YAW_INTEGRAL_LIMIT) {
			_yaw_integral = math::constrain(_yaw_integral, -YAW_INTEGRAL_LIMIT, YAW_INTEGRAL_LIMIT);
		}

		// 微分项（阻尼）
		float d_term = -YAW_KD * current_yaw_rate;

		// PID输出
		yaw_correction = p_term + _yaw_integral + d_term;
		yaw_correction = math::constrain(yaw_correction, -YAW_OUTPUT_LIMIT, YAW_OUTPUT_LIMIT);

		// 输出详细调试信息以诊断问题
		PX4_INFO("PID调试: 目标Y=%.2f 当前Y=%.2f 误差=%.3f 角速度=%.3f P=%.3f I=%.3f D=%.3f 总修正=%.3f 状态=%s",
			 (double)target_yaw, (double)current_yaw, (double)yaw_error,
			 (double)current_yaw_rate, (double)p_term, (double)_yaw_integral, (double)d_term,
			 (double)yaw_correction, has_attitude && has_angular_velocity ? "正常" : "无传感器");

		// 持续PID控制策略 - 只有在明确的yaw旋转指令时才停止自动修正
		float manual_yaw_input = control_sp(ControlAxis::YAW);

		if (fabsf(manual_yaw_input) > YAW_INPUT_THRESHOLD) {
			// 用户明确想要旋转，此时仍保持基础稳定为了更明显的对比效果
			yaw_correction = (p_term * YAW_MANUAL_P_FACTOR) + _yaw_integral + (d_term * YAW_MANUAL_D_FACTOR);  // 使用定义的因子
			PX4_INFO("手动yaw模式: 目标旋转=%.3f, PID修正=%.3f", (double)manual_yaw_input, (double)yaw_correction);
		} else {
			// 无手动yaw控制时，应用最大PID稳定控制
			yaw_correction = (p_term * YAW_AUTO_P_FACTOR) + _yaw_integral + (d_term * YAW_AUTO_D_FACTOR);  // 使用定义的因子
			PX4_INFO("自动稳定模式: 误差=%.3f, PID修正=%.3f (力度增强)", (double)yaw_error, (double)yaw_correction);
		}
	}

	// 初始化执行器输出
	for (size_t i = 0; i < actuator_sp.size(); i++) {
		actuator_sp(i) = 0.0f;
	}

	// 推力控制 (THRUST_Z) - 左摇杆上下
	float thrust_control = control_sp(ControlAxis::THRUST_Z);
	if (thrust_control > 0.0f) {
		// 上升：电机1-2
		actuator_sp(0) = thrust_control;
		actuator_sp(1) = thrust_control;
	} else if (thrust_control < 0.0f) {
		// 下降：电机3-4
		actuator_sp(2) = -thrust_control;
		actuator_sp(3) = -thrust_control;
	}
	// 无手动输入时应用定高控制
	else if (_altitude_hold_enabled && has_valid_position) {
		// 定高控制输出叠加到垂直控制
		if (altitude_throttle_output > 0) {
			// 需要上升
			actuator_sp(0) = altitude_throttle_output; // 电机1
			actuator_sp(1) = altitude_throttle_output; // 电机2
		} else if (altitude_throttle_output < 0) {
			// 需要下降
			actuator_sp(2) = -altitude_throttle_output; // 电机3
			actuator_sp(3) = -altitude_throttle_output; // 电机4
		}
	}

	// 偏航控制 (YAW) - 左摇杆左右，自旋 + 姿态稳定修正
	float yaw_control = control_sp(ControlAxis::YAW) + yaw_correction;
	if (yaw_control > 0.0f) { // 右偏航
		// 右偏航：电机5和6（根据有效性矩阵，产生正偏航力矩）
		actuator_sp(4) = yaw_control;
		actuator_sp(5) = yaw_control;
	} else if (yaw_control < 0.0f) { // 左偏航
		// 左偏航：电机7和8（根据有效性矩阵，产生负偏航力矩）
		actuator_sp(6) = -yaw_control;
		actuator_sp(7) = -yaw_control;
	}

	// 前进/后退控制 (PITCH) - 右摇杆上下
	float pitch_control = control_sp(ControlAxis::PITCH);
	// 左/右移动控制 (ROLL) - 右摇杆左右
	float roll_control = control_sp(ControlAxis::ROLL);

	// 计算各电机的控制值
	float motor5_output = 0.0f;  // 左前
	float motor6_output = 0.0f;  // 右前
	float motor7_output = 0.0f;  // 右后
	float motor8_output = 0.0f;  // 左后

	// 手动控制模式
	bool horizontal_manual_input = (fabsf(pitch_control) > MANUAL_DEADZONE) || (fabsf(roll_control) > MANUAL_DEADZONE);

	if (horizontal_manual_input) {
		// 手动控制模式
		// 前进/后退分量
		if (pitch_control > 0.0f) { // 前进
			motor6_output += pitch_control;  // 右前
			motor8_output += pitch_control;  // 左前
		} else if (pitch_control < 0.0f) { // 后退
			motor5_output += -pitch_control; // 左前
			motor7_output += -pitch_control; // 右后
		}

		// 左/右移动分量
		if (roll_control > 0.0f) { // 右移
			motor6_output += roll_control;   // 右前
			motor7_output += roll_control;   // 右后
		} else if (roll_control < 0.0f) { // 左移
			motor5_output += -roll_control;  // 左前
			motor8_output += -roll_control;  // 左后
		}
	} else if (_position_hold_enabled && has_valid_position) {
		// 位置控制模式
		// 前进/后退分量
		if (position_forward_output > 0.0f) { // 前进
			motor6_output += position_forward_output;  // 右前
			motor8_output += position_forward_output;  // 左前
		} else if (position_forward_output < 0.0f) { // 后退
			motor5_output += -position_forward_output; // 左前
			motor7_output += -position_forward_output; // 右后
		}

		// 左/右移动分量
		if (position_lateral_output > 0.0f) { // 右移
			motor6_output += position_lateral_output;   // 右前
			motor7_output += position_lateral_output;   // 右后
		} else if (position_lateral_output < 0.0f) { // 左移
			motor5_output += -position_lateral_output;  // 左前
			motor8_output += -position_lateral_output;  // 左后
		}
	}

	// 应用电机控制输出
	actuator_sp(4) = motor5_output;  // 电机5 - 左前
	actuator_sp(5) = motor6_output;  // 电机6 - 右前
	actuator_sp(6) = motor7_output;  // 电机7 - 右后
	actuator_sp(7) = motor8_output;  // 电机8 - 左后

	// 限制主电机输出在[0,1]范围内
	for (int i = 0; i < 8; i++) {
		if (actuator_sp(i) < MOTOR_MIN_OUTPUT) actuator_sp(i) = MOTOR_MIN_OUTPUT;
		if (actuator_sp(i) > MOTOR_MAX_OUTPUT) actuator_sp(i) = MOTOR_MAX_OUTPUT;
	}

	// 9号电机固定测试值
	actuator_sp(8) = TEST_MOTOR9_OUTPUT;

	// 基础调试信息
	static int debug_counter = 0;
	if (++debug_counter % ATTITUDE_DEBUG_INTERVAL == 0) {
		PX4_INFO("Hetero10PWM: 姿态修正 R=%.2f P=%.2f Y=%.2f",
			 (double)roll_correction, (double)pitch_correction, (double)yaw_correction);
		debug_counter = 0;
	}
}
